/*
 * Brain.cpp
 *
 *  Created on: 04-01-2011
 *      Author: Piotr
 */

#include "AI/Brain.hpp"
#include "AI/thoughts/MoveThought.hpp"
#include "utils/MathUtils.hpp"
#include "utils/Randomizer.hpp"
#include "world/World.hpp"
#include "world/Barrier.hpp"
#include "AI/fuzzy/FuzzyUtils.hpp"
#include "AI/fuzzyEngine/BarrierMovementEngine.hpp"
#include "AI/thoughts/DistortionMoveThoughtDecorator.hpp"
#include "world/visitors/BlockingElementsGetterVisitor.hpp"
#include "world/visitors/MeatSuppliesGetterVisitor.hpp"
#include "world/visitors/VegetableSuppliesGetterVisitor.hpp"
#include "world/visitors/HerbivoreGetterVisitor.hpp"
#include "world/visitors/CarnivoreGetterVisitor.hpp"
#include "world/visitors/WaterSuppliesGetterVisitor.hpp"
#include "world/creation/MatingSystem.hpp"
using namespace std;
using boost::shared_ptr;

Thought* Brain::getRandomMove(const Genotype& genotype, State& state, float deltaTimeInSeconds)
{
	if(Randomizer::nextInt(2) == 0)
	{
		const int maxAngleChange = genotype.getMaxAngularVelocityInDegreeses();
		float angleInRadiansChange = MathUtils::toRadians((Randomizer::nextInt(maxAngleChange))
				- maxAngleChange / 2);
		float prevAngle = state.getLookDirection().getAngleInRadians();
		return new MoveThought(Vector((cosf(prevAngle + angleInRadiansChange)), (sinf(prevAngle
				+ angleInRadiansChange))), genotype.getMaxMovementSpeed() * deltaTimeInSeconds);
	}
	else
	{
		return new MoveThought(state.getLookDirection(), genotype.getMaxMovementSpeed()
				* deltaTimeInSeconds);
	}
}

Thought* Brain::getEvadingMove(const Vector& position, const vector<Element*>& blockingElements,
		State& state, const Genotype& genotype, float deltaTimeInSeconds)
{
	calculateMassInAngles(position, blockingElements, state, genotype);

	float maxValue = MathUtils::max(3, onTheLeft, inTheCenter, onTheRight);
	float maxSquaredDistance = (genotype.getSightDistance() + Barrier::SIZE)
			* (genotype.getSightDistance() + Barrier::SIZE);
	const float factor = FuzzyUtils::getMembershipValueLeftInf(closestElement - Individual::SIZE, 0,
			maxSquaredDistance);
	const float maxAngleChange = MathUtils::toRadians(genotype.getMaxAngularVelocityInDegreeses());

	BarrierMovementEngine engine = BarrierMovementEngine();
	engine.calculate(onTheLeft, inTheCenter, onTheRight, maxAngleChange, maxValue, closestElement);

	float angleChange = engine.getAngleChange();
	angleChange = factor * factor * factor * factor * angleChange;
	float lookAngle = state.getLookDirection().getAngleInRadians();
	return new MoveThought(Vector((cosf(lookAngle + angleChange)), (sinf(lookAngle + angleChange))),
			genotype.getMaxMovementSpeed() * (1.0f - 2 * factor / 3) * deltaTimeInSeconds
					* engine.getSpeedFactor());
}

Thought* Brain::getEatingMove(Individual& me, const World::ElementsVector& elements,
		float deltaTimeInSeconds)
{
	const Vector& position = me.getPosition();
	State & state = me.getState();
	const Genotype& genotype = me.getGenotype();

	vector<Supply*> supplyElements = getEatingSupplies(genotype.isCarnivore(), elements);
	Vector closestSupply = Vector(2 * genotype.getSightDistance(), 2 * genotype.getSightDistance());
	for(vector<Supply*>::iterator it = supplyElements.begin(); it != supplyElements.end(); ++it)
	{
		const Vector& toSupplyVector = ((*it)->getBounds().getCenter() - position);
		if(closestSupply.getSquaredLength() > toSupplyVector.getSquaredLength())
		{
			closestSupply = toSupplyVector;
		}
		const float distance = toSupplyVector.getLength();
		if(distance <= Individual::SIZE)
		{
			Supply* supply = (*it);
			float prevAmount = supply->getAmount();
			supply->decreaseAmount(State::EATING_SPEED);
			float actualAmount = supply->getAmount();
			state.changeHungerValue(actualAmount - prevAmount);
			return new MoveThought();
		}
	}

	if(supplyElements.size() > 0)
	{
		const Vector& lookDirection = state.getLookDirection();
		const float maxSpeed = genotype.getMaxMovementSpeed();
		const float speed = MathUtils::min(2, closestSupply.getLength(), maxSpeed);
		return new DistortionMoveThoughtDecorator(new MoveThought((closestSupply * 0.1)
				+ (lookDirection * 0.9), speed * deltaTimeInSeconds), 50);
	}
	else if(genotype.isCarnivore())
	{
		HerbivoreGetterVisitor getter;
		getter.visitEveryElement(elements);
		vector<Individual*> herbivores = getter.getVector();
		Vector closestHerbivore = Vector(2 * genotype.getSightDistance(), 2
				* genotype.getSightDistance());
		for(vector<Individual*>::iterator it = herbivores.begin(); it != herbivores.end(); ++it)
		{
			const Vector& toHerbivoreVector = ((*it)->getBounds().getCenter() - position);
			if(closestHerbivore.getSquaredLength() > toHerbivoreVector.getSquaredLength())
			{
				closestHerbivore = toHerbivoreVector;
			}
			const float distance = toHerbivoreVector.getLength();
			if(distance <= Individual::SIZE)
			{
				Individual* herbivore = (*it);
				float prevAmount = herbivore->getState().getLifeValue();
				herbivore->getState().changeLifeValue(State::EATING_SPEED);
				float actualAmount = herbivore->getState().getLifeValue();
				state.changeHungerValue(prevAmount - actualAmount);
				return new MoveThought();
			}
		}

		if(herbivores.size() > 0)
		{
			const Vector& lookDirection = state.getLookDirection();
			const float maxSpeed = genotype.getMaxMovementSpeed();
			const float speed = MathUtils::min(2, closestHerbivore.getLength(), maxSpeed);
			return new DistortionMoveThoughtDecorator(new MoveThought((closestHerbivore * 0.1)
					+ (lookDirection * 0.9), speed * deltaTimeInSeconds), 50);
		}
	}
	return 0;
}

Thought* Brain::getDrinkingMove(Individual& me, const World::ElementsVector& visibleElements,
		float deltaTimeInSeconds)
{
	const Vector& position = me.getPosition();
	State & state = me.getState();
	const Genotype& genotype = me.getGenotype();

	vector<WaterSupply*> waterSupplies;
	findWaterSupplies(waterSupplies, visibleElements);

	Vector closestSupply = Vector(2 * genotype.getSightDistance(), 2 * genotype.getSightDistance());
	for(vector<WaterSupply*>::iterator it = waterSupplies.begin(); it != waterSupplies.end(); ++it)
	{
		const Vector& toSupplyVector = ((*it)->getBounds().getCenter() - position);
		if(closestSupply.getSquaredLength() > toSupplyVector.getSquaredLength())
		{
			closestSupply = toSupplyVector;
		}
		const float distance = toSupplyVector.getLength();
		if(distance <= Individual::SIZE)
		{
			WaterSupply* supply = (*it);
			float prevAmount = supply->getAmount();
			supply->decreaseAmount(State::DRINKING_SPEED);
			float actualAmount = supply->getAmount();
			state.changeThirstValue(actualAmount - prevAmount);
			return new MoveThought();
		}
	}

	if(waterSupplies.size() > 0)
	{
		const Vector& lookDirection = state.getLookDirection();
		const float maxSpeed = genotype.getMaxMovementSpeed();
		const float speed = MathUtils::min(2, closestSupply.getLength(), maxSpeed);
		return new DistortionMoveThoughtDecorator(new MoveThought((closestSupply * 0.1)
				+ (lookDirection * 0.9), speed * deltaTimeInSeconds), 50);
	}

	return 0;
}

Thought* Brain::getMatingMove(Individual& me, const World::ElementsVector& elements,
		float deltaTimeInSeconds)
{
	const Vector& position = me.getPosition();
	State & state = me.getState();
	const Genotype& genotype = me.getGenotype();

	vector<Individual*> matingIndividuals = getMatingIndividuals(genotype.isCarnivore(), elements);
	Vector closestIndividual = Vector(2 * genotype.getSightDistance(), 2
			* genotype.getSightDistance());
	for(vector<Individual*>::iterator it = matingIndividuals.begin(); it != matingIndividuals.end(); ++it)
	{
		const Vector& toIndividualVector = ((*it)->getBounds().getCenter() - position);
		if(closestIndividual.getSquaredLength() > toIndividualVector.getSquaredLength())
		{
			closestIndividual = toIndividualVector;
		}
		const float distance = toIndividualVector.getLength();
		if(distance <= Individual::SIZE)
		{
			MatingSystem matingSystem(me, **it);
			vector<Individual*> children;
			matingSystem.mate(children);

			BOOST_FOREACH(Individual* child, children)
						{
							World::getInstance().scheduleToAdd(child);
						}
			return new MoveThought();
		}
	}

	if(matingIndividuals.size() > 0)
	{
		const Vector& lookDirection = state.getLookDirection();
		const float maxSpeed = genotype.getMaxMovementSpeed();
		const float speed = closestIndividual.getLength() < maxSpeed? closestIndividual.getLength()
				: maxSpeed;
		return new DistortionMoveThoughtDecorator(new MoveThought((closestIndividual * 0.1)
				+ (lookDirection * 0.9), speed * deltaTimeInSeconds), 50);
	}
	return 0;
}

vector<Supply*> Brain::getEatingSupplies(bool isCarnivore, const World::ElementsVector& elements)
{
	vector<Supply*> supplyElements;
	if(isCarnivore)
	{
		MeatSuppliesGetterVisitor suppliesGetter;
		suppliesGetter.visitEveryElement(elements);
		const vector<MeatSupply*> meatSupplies = suppliesGetter.getVector();
		BOOST_FOREACH (MeatSupply* supply, meatSupplies)
					{
						supplyElements.push_back(static_cast<Supply*> (supply));
					}
	}
	else
	{
		VegetableSuppliesGetterVisitor suppliesGetter;
		suppliesGetter.visitEveryElement(elements);
		const vector<VegetableSupply*> vegetableSupplies = suppliesGetter.getVector();
		BOOST_FOREACH(VegetableSupply* supply, vegetableSupplies)
					{
						supplyElements.push_back(static_cast<Supply*> (supply));
					}
	}
	return supplyElements;
}

vector<Individual*> Brain::getMatingIndividuals(bool isCarnivore,
		const World::ElementsVector& elements)
{
	if(isCarnivore)
	{
		CarnivoreGetterVisitor getter;
		getter.visitEveryElement(elements);
		return getter.getVector();
	}
	else
	{
		HerbivoreGetterVisitor getter;
		getter.visitEveryElement(elements);
		return getter.getVector();
	}
}

Thought* Brain::generateThought(Individual& me, const World::ElementsVector& elements,
		float deltaTimeInSeconds)
{
	const Vector& position = me.getPosition();
	State& state = me.getState();
	const Genotype& genotype = me.getGenotype();

	const float speedFactor = sqrtf((State::LIFE_MAX_VALUE - state.getLifeValue())
			/ State::LIFE_MAX_VALUE);

	Thought* move = 0;

	float maxValue = MathUtils::max(3, state.getLibidoValue(), state.getHungerValue(),
			state.getThirstValue());
	if((state.isHungry() && MathUtils::isEqual(maxValue, state.getHungerValue()))
			|| (state.isHungry() && !state.isMatingTime() && !state.isThirsty()))
	{
		move = getEatingMove(me, elements, deltaTimeInSeconds * speedFactor);
		if(move != 0)
		{
			return move;
		}
	}

	if((state.getThirstValue() && MathUtils::isEqual(maxValue, state.getThirstValue()))
			|| (!state.isHungry() && !state.isMatingTime() && state.isThirsty()))
	{
		move = getDrinkingMove(me, elements, deltaTimeInSeconds * speedFactor);
		if(move != 0)
		{
			return move;
		}
	}

	if((state.isMatingTime() && MathUtils::isEqual(maxValue, state.getLibidoValue()))
			|| (!state.isHungry() && state.isMatingTime() && !state.isThirsty()))
	{
		move = getMatingMove(me, elements, deltaTimeInSeconds * speedFactor);
		if(move != 0)
		{
			return move;
		}
	}

	BlockingElementsGetterVisitor getter;
	getter.visitEveryElement(elements);
	const vector<Element*> blockingElements = getter.getVector();

	if(blockingElements.size() == 0)
	{
		move = getRandomMove(genotype, state, deltaTimeInSeconds * speedFactor);
	}
	else
	{
		move = getEvadingMove(position, blockingElements, state, genotype, deltaTimeInSeconds
				* speedFactor);
	}
	return new DistortionMoveThoughtDecorator(static_cast<MoveThought*> (move), 50);
}

void Brain::calculateMassInAngles(const Vector& position, const vector<Element*>& elements,
		State& state, const Genotype& genotype)
{
	float maxSquaredDistance = (genotype.getSightDistance() + Barrier::SIZE)
			* (genotype.getSightDistance() + Barrier::SIZE);

	float lookAngle = state.getLookDirection().getAngleInRadians();
	const float anglePart = MathUtils::toRadians(genotype.getSightRange()) / 3;
	closestElement = maxSquaredDistance;

	onTheLeft = 0.0f;
	inTheCenter = 0.0f;
	onTheRight = 0.0f;
	for(vector<Element*>::const_iterator it = elements.begin(); it != elements.end(); ++it)
	{
		const Vector& elementVector = (*it)->getBounds().getCenter() - position;
		float squaredLength = elementVector.getSquaredLength();
		if(squaredLength < closestElement)
		{
			closestElement = squaredLength;
		}
		float weight = maxSquaredDistance - squaredLength;
		weight = powf(weight, elements.size());
		const float minAngle = lookAngle - 3 * anglePart / 2;
		const Vector& direction = Vector(cosf(minAngle), sinf(minAngle));
		const float acos = acosf(elementVector.getCos(direction));

		onTheLeft += weight * FuzzyUtils::getMembershipValueLeftInf(acos, anglePart / 2, 3 * anglePart
				/ 2);
		inTheCenter += weight * FuzzyUtils::getMembershipValue(acos, anglePart / 2, 5 * anglePart / 2);
		onTheRight += weight * FuzzyUtils::getMembershipValueRightInf(acos, 3 * anglePart / 2, 5
				* anglePart / 2);
	}
}

void Brain::findWaterSupplies(vector<WaterSupply*>& waterSupplies,
		const World::ElementsVector& elements)
{
	WaterSuppliesGetterVisitor waterSuppliesGetter;
	waterSuppliesGetter.visitEveryElement(elements);

	waterSupplies = waterSuppliesGetter.getVector();
}
